#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Pose.h>
#include <rocr6_msgs/Move.h>
#include <std_msgs/UInt16.h>
#include <math.h>
#include <innfos_vision/ik6s.h>
#include <innfos_vision/priority_path.h>
#include <innfos_vision/rocr6-ctrl.h>
#include <csignal> 
#include <ecl/threads.hpp>

//定义的宏
#define PI 3.1415926535897931
#define DATA_GAIN  10
#define EPSINON 1e-4  // 精度，小于它就认为是0
#define ALLOW_MAX_DIFF (1 / 180 * PI)  /* 距离上一轴角度的最大允许偏差 */
#define JOINT_MAX   6

double xlast[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double ax[6];
double itheta[48];
static int gn = 1;      //逆解组号
static int dir = 0;

using namespace std;
// double dh_ptr[] =
// {
// 	0.1215,		0,			0,			0.1105,			0.090,			0.082,
// 	0,			0,			-0.300,		-0.276,			0,				0,
// 	0,			PI / 2,	0,			0,				PI / 2,		-PI / 2,
// 	0,			-PI / 2,	0,			-PI / 2,		0,				0
// };

double dh_ptr[] =
{
	0.0792,		0,			0,			0.0792,			0.0792,			0.0792,
	0,			0,			-0.173,		-0.173,			0,				0,
	0,			PI / 2,	0,			0,				PI / 2,		-PI / 2,
	0,			-PI / 2,	0,			-PI / 2,		0,				0
};

//innfos_vision类
class innfos_vision
{
    //私有权限
private:
    /* data */
    ros::NodeHandle n;
    void callback(const geometry_msgs::Pose::ConstPtr &msg);
    void input(const std_msgs::Float64::ConstPtr &msg);
    double Roll,Pitch,Yaw;
    double a1,a2,a3,a4,a5,a6;
    ros::Publisher pub1;
    ros::Publisher pub2;
    ros::Publisher pub3;
    ros::Publisher pub4;
    ros::Publisher pub5;
    ros::Publisher pub6;
    ros::Subscriber sub_vi;
    ros::Subscriber sub_input;
    ros::Publisher pub_xiqi;
    //定义/gluon/joint_1_position每个关节角
    std_msgs::Float64 msg1;
    std_msgs::Float64 msg2;
    std_msgs::Float64 msg3;
    std_msgs::Float64 msg4;
    std_msgs::Float64 msg5;
    std_msgs::Float64 msg6;
    //吸气的消息
    std_msgs::UInt16 msg_xi;

    ecl::Thread thread_;
    int loop_rate_;
    void thread_func();
    double val,sel;
    int active;

    

    //公共权限
public:
    innfos_vision(/* args */);
    void loop();
};
//行为
innfos_vision::innfos_vision(/* args */)
{
    sub_vi = n.subscribe<geometry_msgs::Pose>("/rocr6_msgs/Move",10,&innfos_vision::callback,this);
    sub_input = n.subscribe<std_msgs::Float64>("/vision_innfos/input",10,&innfos_vision::input,this);
    //获取参数
    n.getParam("/innfos_vision/Roll",Roll);
    n.getParam("/innfos_vision/Pitch",Pitch);
    n.getParam("/innfos_vision/Yaw",Yaw);

    n.getParam("/innfos_vision/a1",a1);
    n.getParam("/innfos_vision/a2",a2);
    n.getParam("/innfos_vision/a3",a3);   
    n.getParam("/innfos_vision/a4",a4);
    n.getParam("/innfos_vision/a5",a5);
    n.getParam("/innfos_vision/a6",a6);   
    //发布6个关节的位姿
    loop_rate_ = 10;
    pub1 = n.advertise<std_msgs::Float64>("/gluon/joint_1_position",10);
    pub2 = n.advertise<std_msgs::Float64>("/gluon/joint_2_position",10);
    pub3 = n.advertise<std_msgs::Float64>("/gluon/joint_3_position",10);
    pub4 = n.advertise<std_msgs::Float64>("/gluon/joint_4_position",10);
    pub5 = n.advertise<std_msgs::Float64>("/gluon/joint_5_position",10);
    pub6 = n.advertise<std_msgs::Float64>("/gluon/joint_6_position",10);
    //发布吸气话题
    pub_xiqi = n.advertise<std_msgs::UInt16>("/zmotor/state", 100);

    thread_.start(ecl::generateFunctionObject(&innfos_vision::thread_func,*this));
    while (ros::ok())
    {
      /* code */
      if (active)
      {
        /* code */
     pub1.publish(msg1);
     pub2.publish(msg2);
     pub3.publish(msg3);
     pub4.publish(msg4);  
     pub5.publish(msg5);
     pub6.publish(msg6);   
      }
      ros::spinOnce();  
    } 
}
/* 手动控制机械臂
* @data :0 去拍照位    单位：弧度
* @data :1 去抓取位
* @data :2 回零位
*/
void innfos_vision::input(const std_msgs::Float64::ConstPtr &msg)
{
  static int first = 0;
  if(msg->data == 0)
  {
    msg1.data = 1.57;
    msg2.data = 0;
    msg3.data = -0.52;
    msg4.data = 1.047;
    msg5.data = -1.57;
    msg6.data = 0;
    active = 1;
    cout <<"**************************去拍照位*********************************"<<endl;
    }
    else if (msg->data == 1)
    {
      /* code */
      for (int i = 0; i < 1; i++)
      {
        /* code */
        msg1.data = xlast[0];
        msg2.data = xlast[1];
        msg3.data = xlast[2];
        msg4.data = xlast[3];
        msg5.data = xlast[4];
        msg6.data = xlast[5];

        pub1.publish(msg1);
        pub2.publish(msg2);
        pub3.publish(msg3);
        pub4.publish(msg4);  
        pub5.publish(msg5);
        pub6.publish(msg6); 
        active = 1;
        cout  << "*************************去抓取********************************"<<endl;
        msg_xi.data = 500;
        pub_xiqi.publish(msg_xi);
        sleep(10);
        cout  << "*************************吸气**********************************"<<endl;
      }
    }
    else if (msg->data == 2)
    {
      /* code */
    msg1.data = 0;
    msg2.data = 0;
    msg3.data = 0;
    msg4.data = 0;
    msg5.data = 0;
    msg6.data = 0;
    active = 1;
    cout <<"***************************回零***********************************"<<endl;
    }
    else if (msg->data == 3)
    {
      /* code */
    msg1.data = 1.57;
    msg2.data = 0;
    msg3.data = -0.52;
    msg4.data = 1.047;
    msg5.data = -1.57;
    msg6.data = 0;
    active = 1;
    cout <<"***************************去拍照位******************************"<<endl;
    sleep(10);
    for(int i=0;i<1;i++)
    {
     msg1.data = xlast[0];
     msg2.data = xlast[1];
     msg3.data = xlast[2];
     msg4.data = xlast[3];
     msg5.data = xlast[4];
     msg6.data = xlast[5];

     pub1.publish(msg1);
     pub2.publish(msg2);
     pub3.publish(msg3);
     pub4.publish(msg4);  
     pub5.publish(msg5);
     pub6.publish(msg6); 
     active = 1;
     cout  << "***************************去抓取********************************"<<endl;
     msg_xi.data = 500;
     pub_xiqi.publish(msg_xi);
     sleep(10);
     cout  << "***************************吸气**********************************"<<endl;
    }
    sleep(10);
    msg1.data = 0;
    msg2.data = 0;
    msg3.data = 0;
    msg4.data = 0;
    msg5.data = 0;
    msg6.data = 0;
    active = 1;
    cout <<"***************************回零**********************************"<<endl;
    }else if (msg->data == 4)
    {
      /* code */
    msg1.data = a1;
    msg2.data = a2;
    msg3.data = a3;
    msg4.data = a4;
    msg5.data = a5;
    msg6.data = a6;
    active = 1;
    cout <<"*****************************去抓取位****************************"<<endl;
    }
    else 
    {
      active = 0;
    }
}

void innfos_vision::thread_func()
{
    ros::Rate rate(loop_rate_);
    while(ros::ok())
    {
	//printf("while...\n");
        rate.sleep();
    }
}

void innfos_vision::loop()
{
   ros::spin();
}
/* 逆解函数
* @Joint 6轴各关节值结构体，单位弧度
* @lastJoints 上一次计算的关节值结构体
* @pose 要解逆解的位姿矩阵
*/
void innfos_vision::callback(const geometry_msgs::Pose::ConstPtr &msg)
{
    int rst = 0;
    ax[0] = msg->position.x;
    ax[1] = msg->position.y;
    ax[2] = msg->position.z;
    ax[3] = Roll;
    ax[4] = Pitch;
    ax[5] = Yaw;
  
    rst = ik6s(ax, dh_ptr, itheta);
    if(rst)
    {
      return;
    }
    else
    {
      priority_path(dir,itheta,xlast, &val, &sel);
      cout << "priority_path:"<<priority_path<<endl;

      if((val==0)&&(sel==0))
      {
        printf("没有满足条件的最优解. \r\n");
        return;
      }
      if(sel==0)
      {
        gn = 0;
      }
      else
      {
        gn = sel-1;
      }
      printf("运动方向：[%d] 最小路径权重：[%f]->选择第[%d]组解\r\n",dir, val,(int)sel);
      xlast[0] = itheta[gn+0];
      xlast[1] = itheta[gn+8];
      xlast[2] = itheta[gn+16];
      xlast[3] = itheta[gn+24];
      xlast[4] = itheta[gn+32];
      xlast[5] = itheta[gn+40];

      printf("%f %f %f %f %f %f\r\n", 
            xlast[0], 
            xlast[1], 
            xlast[2], 
            xlast[3],
            xlast[4], 
            xlast[5]
      );
    }
}
/* 主函数
* @使用new开辟新的内存
* @使用进程间通讯
* @把innfos_vision()放在单独的内存中运行
*/
boost::shared_ptr<innfos_vision> node_;
int main(int argc,char** argv)
{
    ros::init(argc, argv, "innfos_vision");
    ros::NodeHandle nh;

    ROS_INFO("innfos_vision start!!!");
    node_.reset(new innfos_vision());
    node_->loop();
    ROS_INFO( "innfos_vision stop!!!");
    return 0;

}





